package com.cgw.room;

import android.annotation.SuppressLint;
import android.content.Context;
import android.content.Intent;
import android.graphics.Point;
import android.graphics.PointF;
import android.net.ConnectivityManager;
import android.net.Network;
import android.net.NetworkCapabilities;
import android.net.NetworkInfo;
import android.net.NetworkRequest;
import android.net.wifi.WifiInfo;
import android.net.wifi.WifiManager;
import android.os.Build;
import android.os.Bundle;
import android.os.Handler;
import android.provider.Settings;
import android.util.Log;
import android.view.View;
import android.view.WindowManager;
import android.widget.Button;
import android.widget.TextView;

import androidx.annotation.Nullable;
import androidx.appcompat.app.AppCompatActivity;
import androidx.fragment.app.FragmentTransaction;

import com.cgw.room.socket.HeartBeatTimer;
import com.cgw.room.util.BufferUtil;
import com.cgw.room.util.FileOperator;
import com.cgw.room.util.HexUtils;
import com.cgw.room.util.Util;
import com.cgw.room.widget.AccButton;
import com.cgw.room.widget.BaseFragment;
import com.cgw.room.widget.RockerButtonView;
import com.cgw.room.widget.RockerSafeView;

import java.io.File;
import java.io.IOException;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetAddress;
import java.net.SocketException;
import java.net.UnknownHostException;
import java.util.HashMap;
import java.util.Map;
import java.util.Timer;
import java.util.TimerTask;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;

public class DogControllerActivity extends AppCompatActivity {
    // # part 1: send to robot
    private static final int INST_HEARTBEATS = 0x2104_0001           ;
    private static final int INST_HEARTBEATS_2ND = 0x2102_0001       ;
    private static final int INST_STAND_UP_DOWN = 0x2101_0202        ;
    private static final int INST_FORCE_CONTROL_MODE = 0x2101_020A   ;
    private static final int INST_MOTION_START_STOP = 0x2101_0201    ;
    private static final int INST_WALK_X = 0x2101_0130               ;
    private static final int INST_WALK_Y = 0x2101_0131               ;
    private static final int INST_YAW = 0x2101_0135                  ;
    private static final int INST_SOFT_STOP = 0x2101_0C0E            ;

    private static final int INST_GAIT_WALK = 0x2101_0300;
    private static final int INST_GAIT_RUN  = 0x2101_0307; // Not used

    private static final int INST_GAIT_L_WALK = 0x2101_0420;
    private static final int INST_GAIT_SHANDI = 0x2101_0421;
    private static final int INST_GAIT_QUIET = 0x2101_0422; // Not used
    private static final int INST_GAIT_STAIRS = 0x2101_0405;
    private static final int INST_GAIT_EXIT_RL = 0x2101_0202;

    private static final int INST_MANUAL_MODE = 0x2101_0C02;
    private static final int INST_NON_MANUAL_MODE = 0x2101_0C03;

    private static final int INST_AVOIDANCE_MODE = 0x2101_EE02; // Note: to Sensor Host 43899. Return 0x3100_EE02

    // Note: get new status from Sensor Host
    private static final int INST_STATE_AVOIDANCE_MODE = 0x3101_EE02;
    private static final int INST_STATE_SPEED_INPUT_SRC = 0x3100_EE03; // from Sensor Host 43899
    private static final int INST_STATE_MAP_MODE = 0x3100_EE01; // from Sensor Host 43899
    private static final int INST_STATE_CHARGE_CHECK = 0x9191_0253; // from Sensor Host


    //# part 2: receiver from robot
    private static final int INST_STATE_RCSDATA = 0x0000_1008           ;
    private static final int INST_STATE_MOTIONSTATEDATA = 0x0000_1009   ;
    private static final int INST_STATE_CTRLSENSORDATA = 0x0000_100A    ;
    private static final int INST_STATE_CTRLSAFEDATA = 0x0000_100B      ;
    private static final int INST_STATE_ROBOTDATA = 0x0000_0906         ;
    private static final int INST_STATE_BATTERYDATA = 0x2105_0F0A       ;
    // CGW BACKUP
    private static final String TAG = DogControllerActivity.class.getSimpleName();
    private Button mBtnNet = null;
    private Button mBtnStatus = null;
    private AccButton mBtnTurnLeft = null;
    private AccButton mBtnTurnRight = null;
    private Button mBtnStop = null;
    private Button mBtnConnect = null;
    private Button mBtn1 = null;
    private Button mBtn2 = null;
    private Button mBtn3 = null;

    private Button mBtn_manual = null;
    private Button mBtn_avoid = null;

    private Button mBtn4 = null;
    private Button mBtnWifi = null;
    private Button mBtnBattery = null;
    private TextView mTextBattery = null;
    private TextView mTextDelay = null;
    private TextView mTextLoss = null;
    private RockerSafeView mRockerViewDirction = null;
    private RockerButtonView mRockerViewState = null;
    private DatagramSocket mSendSocket = null;
    private DatagramSocket mSendSocket_AvoidMode = null;
    private DatagramSocket mRecvSocket = null;
    private ExecutorService mThreadPool;
    private HeartBeatTimer mHeartBeatTimer = null;
    private Runnable mRecvThread = null;
    private boolean mRecieving = true;
    private NetFragment mFragmentNet = null;
    private StatusFragment mFragmentStatus = null;
    Timer timer = null;

    private Map<Integer, Integer> mSendStatistics = new HashMap<Integer, Integer>();
    private long mRecvTimestamp = 0;

    private int mCtrlMode = 0; // 0: manual mode, 1: non-manual mode
    private int mAvoidMode = 1; // 1: stop mode, 2: avoid mode
    private static final int ACCELERATION_TIMER_PERIOD = 20;
    ConnectivityManager mConnectivityManager = null;

    private int mLastLoss = 0;
    private int mTotalLoss = 0;

    private ConnectivityManager.NetworkCallback mNetworkCallback = new ConnectivityManager.NetworkCallback() {
        @Override
        public void onAvailable(Network network) {
            super.onAvailable(network);
            Log.i("[WIFI]", "Connected.");
            createSocket();
            createWifiTimer();
        }

        @Override
        public void onLost(Network network) {
            super.onLost(network);
            Log.i("[WIFI]", "Lost Connected.");
            MainApp.toast("网络连接断开。", 2000);
        }
    };
    Handler mAccHandler = new Handler();
    Runnable mAccRunnable = new Runnable() {
        @SuppressLint({"SetTextI18n", "UseCompatLoadingForDrawables"})
        @Override
        public void run() {
            // TODO_MOYANG
            // 根据是否按下、Rank、方向计算需要发送的值


            long ts = System.currentTimeMillis();
            // 摇杆
            {
                boolean isMotion = mRockerViewDirction.isMotion();
                if (isMotion) {
                    int rank = mRockerViewDirction.getRank(ts);
                    PointF pt = mRockerViewDirction.getDeriction();
                    //sendMessage(INST_YAW, 28212+2000, 0x00, null);

                    int x_val = Math.abs(pt.x) < 0.34 ? 0 : pt.x > 0 ? 6553 + (int)(26200 * pt.x * rank / 5) :
                            -6553 + (int)(26200 * pt.x * rank / 5);
                    int y_val = Math.abs(pt.y) < 0.34 ? 0 : pt.y > 0 ? 24576 + (int)(8500 * pt.y * rank / 5) :
                            -24576 + (int)(8500 * pt.y * rank / 5);

                    sendMessage(INST_WALK_X, x_val, 0x00, null);
                    sendMessage(INST_WALK_Y, y_val, 0x00, null);

                    Log.i("[MOT]", "Rocker = " + rank + "    " + pt);
                }
            }

            // 左右键
            {
                boolean isMotionm_TurnLeft = mBtnTurnLeft.isMotion();
                if (isMotionm_TurnLeft) {
                    int rankLeft = mBtnTurnLeft.getRank(ts); // from 1 to 5
                    Log.i("[MOT]", "Left = " + rankLeft);
                    int yaw_val = -28212-(int)(4000 * (rankLeft) / 5.0);
                    sendMessage(INST_YAW, yaw_val, 0x00, null); // TODO
                }else{
                    boolean isMotionm_TurnRight = mBtnTurnRight.isMotion();
                    if (isMotionm_TurnRight) {
                        int rankRight = mBtnTurnRight.getRank(ts);
                        Log.i("[MOT]", "Right = " + rankRight);
                        int yaw_val = 28212 + (int)(4000 * (rankRight) / 5.0);
                        sendMessage(INST_YAW, yaw_val, 0x00, null); //TODO
                    }
                }
            }

            // 检测连接
            {
                // TODO_MOYANG, 1秒钟不到包
                if (mRecvTimestamp != 0 && ts - mRecvTimestamp > 1000){
                    DogControllerActivity.this.runOnUiThread(() -> {
                        MainApp.toast("连接断开。", 3000);
                        mBtnConnect.setBackground(getResources().getDrawable(R.drawable.unconnected));
                    });
                }
                else if (mRecvTimestamp != 0) {
                    DogControllerActivity.this.runOnUiThread(() -> {
                        mBtnConnect.setBackground(getResources().getDrawable(R.drawable.connected));

                        String s = "Delay from last receive : "
                                + (ts - mRecvTimestamp) + "ms";

                        for (Integer key : mSendStatistics.keySet()){
                            String hex = Integer.toHexString(key);
                            s += hex;
                            s += " : ";
                            s += mSendStatistics.get(key);
                        }
                        mTextDelay.setText(s);
                    });
                }

                DogControllerActivity.this.runOnUiThread(() -> {
                    mTextLoss.setText("LastLoss : " + mLastLoss + "  TotalLoss : " + mTotalLoss);
                });
            }

            mAccHandler.postDelayed(mAccRunnable, ACCELERATION_TIMER_PERIOD); // 每隔1毫秒执行一次
        }
    };
    @SuppressLint("CutPasteId")
    @Override
    protected void onCreate(@Nullable Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_dogcontroller);
        getWindow().setSoftInputMode(WindowManager.LayoutParams.SOFT_INPUT_STATE_ALWAYS_HIDDEN | WindowManager.LayoutParams.SOFT_INPUT_STATE_HIDDEN);

        View decorView = getWindow().getDecorView();
        decorView.setSystemUiVisibility(
                View.SYSTEM_UI_FLAG_HIDE_NAVIGATION
                        | View.SYSTEM_UI_FLAG_FULLSCREEN
                        | View.SYSTEM_UI_FLAG_IMMERSIVE_STICKY);


        String dirPath = MainApp.getProductDir();
        Util.mkdirs(dirPath);

//        if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.M) {
//            String[] permissions = {Manifest.permission.WRITE_EXTERNAL_STORAGE
//                    , Manifest.permission.READ_EXTERNAL_STORAGE
//                    , Manifest.permission.ACCESS_COARSE_LOCATION};
//            for (String str : permissions) {
//                if (checkSelfPermission(str) != PackageManager.PERMISSION_GRANTED) {
//                    requestPermissions(permissions, 111);
//                    break;
//                }
//            }
//        }
//
//        if (ContextCompat.checkSelfPermission(DogControllerActivity.this, Manifest.permission.ACCESS_FINE_LOCATION) != PackageManager.PERMISSION_GRANTED) {
//            ActivityCompat.requestPermissions(DogControllerActivity.this, new String[]{Manifest.permission.ACCESS_FINE_LOCATION}, 111);
//        }

        mBtnStop = findViewById(R.id.btn_stop);
        mBtnStop.setOnClickListener(view -> {
            if (MainApp.checkSendIP())
                sendMessage(INST_SOFT_STOP, 0x00, 0x00, null);
        });
        mBtnConnect = findViewById(R.id.btn_connect);
        mBtnNet = findViewById(R.id.btn_setting);
        mBtnNet.setOnClickListener(view -> {
            setFragmentVisible(mFragmentNet,true);
        });
        mBtnStatus = findViewById(R.id.btn_status);
        mBtnStatus.setOnClickListener(view -> {
            setFragmentVisible(mFragmentStatus,true);
            long ts = System.currentTimeMillis();
            String s = "Delay from last receive : "
                    + (ts - mRecvTimestamp) + "ms  ";

            for (Integer key : mSendStatistics.keySet()){
                String hex = Integer.toHexString(key);
                s += hex;
                s += " : ";
                s += mSendStatistics.get(key);
                s += "  ";
            }
            mFragmentStatus.setStatisticsString(s);
        });
        mBtnTurnLeft = findViewById(R.id.btn_turn_left);
        mBtnTurnLeft.setButtonName("Left");
        //mBtnTurnLeft.setOnClickListener(view -> {
        //    if (MainApp.checkSendIP())
        //        sendMessage(INST_YAW, -(28212+2000), 0x00, null);
        //});
        mBtnTurnRight = findViewById(R.id.btn_turn_right);
        mBtnTurnRight.setButtonName("Right");
        //mBtnTurnRight.setOnClickListener(view -> {
        //    if (MainApp.checkSendIP())
        //        sendMessage(INST_YAW, 28212+2000, 0x00, null);
        //});
        mBtn1 = findViewById(R.id.btn_1);
        mBtn1.setOnClickListener(view -> {
            // TODO_MOYANG
            if (MainApp.checkSendIP())
                sendMessage(INST_GAIT_L_WALK, 0, 0x00, null);
        });
        mBtn2 = findViewById(R.id.btn_2);
        mBtn2.setOnClickListener(view -> {
            // TODO_MOYANG
            if (MainApp.checkSendIP())
                sendMessage(INST_GAIT_SHANDI, 0, 0x00, null);
        });
        mBtn3 = findViewById(R.id.btn_stairs);
        mBtn3.setOnClickListener(view -> {
            // TODO_MOYANG
            if (MainApp.checkSendIP())
                sendMessage(INST_GAIT_STAIRS, 0, 0x00, null);
        });
        mBtn4 = findViewById(R.id.btn_4);
        mBtn4.setOnClickListener(view -> {
            // TODO_MOYANG
            if (MainApp.checkSendIP())
                sendMessage(INST_GAIT_EXIT_RL, 0, 0x00, null);
        });

        mBtn_manual = findViewById(R.id.btn_manual);
        mBtn_manual.setOnClickListener(view -> {
            // TODO_MOYANG
            if (MainApp.checkSendIP()) {
                if (mCtrlMode == 0) {// manual mode
                    sendMessage(INST_NON_MANUAL_MODE, 0, 0x00, null);
                    mCtrlMode = 1; // switch to non manual mode
                }
                else {
                    sendMessage(INST_MANUAL_MODE, 0, 0x00, null);
                    mCtrlMode = 0; // switch back to default manual mode
                }
            }
        });
        mBtn_avoid = findViewById(R.id.btn_avoid);
        mBtn_avoid.setOnClickListener(view -> {
            if (MainApp.checkSendIP_AvoidMode()) {
                if (mAvoidMode == 0x01) mAvoidMode = 0x02;
                if (mAvoidMode == 0x02) mAvoidMode = 0x01;
                sendMessage_AvoidMode(0x3101EE02, mAvoidMode, 0x00, null);
            }
        });


        mRockerViewDirction = findViewById(R.id.rockerSafeView_direction);
//        mRockerViewDirction.setButtonNames(new String[]{"后退","左走","前进","右走"});
//        mRockerViewDirction.setOnClickListener(view -> {
//            if (MainApp.checkSendIP())
//                sendMessage(0x31010201, 0x00, 0x00, null);
//        });
        mRockerViewState = findViewById(R.id.rockerSafeView_state);
        mRockerViewState.setButtonNames(new String[]{"起立/坐下","启停","力控","行走"});
//        mRockerViewState.setOnClickListener(view -> {
//            if (MainApp.checkSendIP())
//                sendMessage(0x31010201, 0x00, 0x00, null);
//        });

        mThreadPool = Executors.newFixedThreadPool(10);
        mHeartBeatTimer = new HeartBeatTimer();

        mHeartBeatTimer.setOnFirstScheduleListener(() -> {
            int code = INST_HEARTBEATS;
            int value = 0;
            int type = 0;
            sendMessage(code, value, type, null);
        });
        mHeartBeatTimer.setOnSecondScheduleListener(() -> {
            int code = INST_HEARTBEATS_2ND;
            int value = 0;
            int type = 0;
            sendMessage(code, value, type, null);
        });
        mBtnWifi = findViewById(R.id.btn_wifi);
        mBtnWifi.setOnClickListener(view -> {
            Intent intent = new Intent(Settings.ACTION_WIFI_SETTINGS);
            startActivity(intent);
        });
        mTextBattery = findViewById(R.id.btn_battary_text);
        mBtnBattery = findViewById(R.id.btn_battary);
        mTextDelay = findViewById(R.id.text_delay);
        mTextLoss = findViewById(R.id.text_loss);

        readIP();
        Log.i("[NULL]", "createSocket");

        mFragmentNet = (NetFragment)(getSupportFragmentManager().findFragmentById(R.id.fragment_net));
        mFragmentStatus = (StatusFragment)(getSupportFragmentManager().findFragmentById(R.id.fragment_status));
        setFragmentVisible(mFragmentNet,false);
        setFragmentVisible(mFragmentStatus,false);

        mAccHandler.postDelayed(mAccRunnable, ACCELERATION_TIMER_PERIOD); // 每隔1毫秒执行一次

        mConnectivityManager = (ConnectivityManager) getSystemService(Context.CONNECTIVITY_SERVICE);
        NetworkRequest.Builder builder = new NetworkRequest.Builder();
        builder.addTransportType(NetworkCapabilities.TRANSPORT_WIFI); // 只监听WiFi网络状态变化
        mConnectivityManager.registerNetworkCallback(builder.build(), mNetworkCallback);
    }

    @SuppressLint("CutPasteId")
    @Override
    protected void onResume(){
        super.onResume();
        Log.i("[Route]", "onResume");
    }

    @SuppressLint("CutPasteId")
    @Override
    protected void onDestroy() {
        super.onDestroy();
        mRecieving = false;
        mHeartBeatTimer.exit();
        Log.i("[Route]", "onDestroy");
        mConnectivityManager.unregisterNetworkCallback(mNetworkCallback);

        if (mSendSocket != null) {
            mSendSocket.close();
            mSendSocket = null;
        }
        if (mSendSocket_AvoidMode != null) {
            mSendSocket_AvoidMode.close();
            mSendSocket_AvoidMode = null;
        }
        if (mRecvSocket != null) {
            mRecvSocket.close();
            mRecvSocket = null;
        }
    }
    void createWifiTimer(){
        timer = new Timer();
        timer.schedule(new TimerTask() {
            @Override
            public void run() {
                WifiManager wifiManager = (WifiManager) getSystemService(Context.WIFI_SERVICE);
                WifiInfo wifiInfo = wifiManager.getConnectionInfo();
                int rssi = wifiInfo.getRssi();
                int level = WifiManager.calculateSignalLevel(rssi, 5); // 这里5是信号等级的数量，可以根据需要调整
                mBtnWifi.setText("Wifi : " + level);
            }
        }, 5000);
    }
    void readIP(){
        String dirPath = MainApp.getProductDir();
        String filePath = dirPath + "/IP_PORT";
        File f = new File(filePath);
        Log.i("[IP]", filePath);
        if (!f.exists())
            return;

        Log.i("[IP]", "exists");
        String fileContent = FileOperator.readString(filePath);
        Log.i("[IP]", "fileContent = " + fileContent);
        String[] IP_Port = fileContent.split(":");
        if (IP_Port.length != 2) {
            MainApp.toast("读取IP格式错误。", 2000);
            return;
        }

        String IP = IP_Port[0];
        MainApp.PORT = IP_Port[1];
        String[] IPSegments = IP.split("\\.");

        Log.i("[IP]", "IP = " + IP);
        Log.i("[IP]", "PORT = " + MainApp.PORT + " Len = " + IPSegments.length);
        if (IPSegments.length != 4) {
            MainApp.toast("读取IP格式错误。", 2000);
            return;
        }

        MainApp.IP_1 = IPSegments[0];
        MainApp.IP_2 = IPSegments[1];
        MainApp.IP_3 = IPSegments[2];
        MainApp.IP_4 = IPSegments[3];
        Log.i("[IP]", "IP = " + MainApp.IP_1 + MainApp.IP_2 + MainApp.IP_3 + MainApp.IP_4 + MainApp.PORT);
    }
    void createSocket(){
        ConnectivityManager cm = (ConnectivityManager) getSystemService(Context.CONNECTIVITY_SERVICE);
        Network[] networks = cm.getAllNetworks();
        Network networkTarget = null;
        for (Network network : networks){
            NetworkInfo netInfo = cm.getNetworkInfo(network);
            if (netInfo == null)
                continue;

            if (netInfo.getType() == ConnectivityManager.TYPE_WIFI){
                networkTarget = network;
            }
        }
        if (networkTarget == null){
            MainApp.toast("当前使用的网络不是Wifi，请注意。", 2000);
        }

        if (networkTarget != null) {
            try {
                // 将socket绑定到特定网络
                if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.M) {
                    cm.bindProcessToNetwork(networkTarget);
                } else if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.LOLLIPOP) {
                    ConnectivityManager.setProcessDefaultNetwork(networkTarget);
                }
            }catch (Exception e){
                e.printStackTrace();
            }
        }

        try {
            if (mSendSocket != null)
                mSendSocket.close();
            if (mSendSocket_AvoidMode != null)
                mSendSocket_AvoidMode.close();

            if (MainApp.PORT.isEmpty()) {
                mSendSocket = new DatagramSocket();
            }else {
                mSendSocket = new DatagramSocket(Integer.parseInt(MainApp.PORT));
                mSendSocket_AvoidMode = new DatagramSocket(Integer.parseInt(MainApp.PORT_AVOIDMODE));
            }
            if (mSendSocket != null){ // by chenmy open
                Log.i("[NULL]", mSendSocket.toString());
                mSendSocket.setReuseAddress(true);
                if (MainApp.checkSendIP()){
                    startHeartbeatTimer();
                }
            }else{
                Log.i("[NULL]", "null");
            }

            if (mRecvSocket != null)
                mRecvSocket.close();
            mRecvSocket = new DatagramSocket(Integer.parseInt(MainApp.PORT_RECV));
            mRecvSocket.setReuseAddress(true);
            mRecvSocket.setSoTimeout(6000);
            if (mRecvSocket != null){ // by chenmy open
                new Thread(new RecvUDPThread()).start();
            }
        } catch (SocketException e) {
            MainApp.toast(e.toString(), 2000);
            e.printStackTrace();
        }
    }
    private byte[] cmd2Byte(int code, int value, int type, byte[] data){
        byte[] byteCode = BufferUtil.int2byte_LittleEndian(code);
        byte[] byteValue = BufferUtil.int2byte_LittleEndian(value);
        byte[] byteType = BufferUtil.int2byte_LittleEndian(type);

        int sendLen = 12;
        int dataLen = 0;
        if (type == 0x01){
            dataLen = (data == null ? 0 : data.length);
        }
        sendLen += dataLen;
        byte[] byteSend = new byte[sendLen];
        byteSend[0] = byteCode[0];
        byteSend[1] = byteCode[1];
        byteSend[2] = byteCode[2];
        byteSend[3] = byteCode[3];
        byteSend[4] = byteValue[0];
        byteSend[5] = byteValue[1];
        byteSend[6] = byteValue[2];
        byteSend[7] = byteValue[3];
        byteSend[8] = byteType[0];
        byteSend[9] = byteType[1];
        byteSend[10] = byteType[2];
        byteSend[11] = byteType[3];

        if (data != null) {
            System.arraycopy(data, 0, byteSend, 12, dataLen);
        }
        return byteSend;
    }
    public void sendMessage(int code, int value, int type, byte[] data) {
        byte[] byteSend = cmd2Byte(code, value, type, data);

        if (mSendStatistics.containsKey(code)){
            mSendStatistics.put(code, mSendStatistics.get(code) + 1);
        }else{
            mSendStatistics.put(code, 1);
        }
//        Log.i("[Order]", Integer.toHexString(byteSend[0] & 0xff));
//        Log.i("[Order]", Integer.toHexString(byteSend[1] & 0xff));
//        Log.i("[Order]", Integer.toHexString(byteSend[2] & 0xff));
//        Log.i("[Order]", Integer.toHexString(byteSend[3] & 0xff));
//        Log.i("[Order]", Integer.toHexString(byteSend[4] & 0xff));
//        Log.i("[Order]", Integer.toHexString(byteSend[5] & 0xff));
//        Log.i("[Order]", Integer.toHexString(byteSend[6] & 0xff));
//        Log.i("[Order]", Integer.toHexString(byteSend[7] & 0xff));
//        Log.i("[Order]", Integer.toHexString(byteSend[8] & 0xff));
//        Log.i("[Order]", Integer.toHexString(byteSend[9] & 0xff));
//        Log.i("[Order]", Integer.toHexString(byteSend[10] & 0xff));
//        Log.i("[Order]", Integer.toHexString(byteSend[11] & 0xff));

        mThreadPool.execute(new Runnable() {
            @Override
            public void run() {
                try {
                    InetAddress targetAddress = InetAddress.getByName(MainApp.getIP());
                    DatagramPacket packet = new DatagramPacket(byteSend, byteSend.length, targetAddress, Integer.valueOf(MainApp.PORT));
                    mSendSocket.send(packet);
                    Log.d(TAG, "数据发送成功 : " + targetAddress + " : " + MainApp.PORT + "----" + HexUtils.encodeHexStr(byteSend));
                    mLastLoss = 0;
                } catch (UnknownHostException e) {
                    e.printStackTrace();
                    ++mTotalLoss;
                    ++mLastLoss;
                } catch (IOException e) {
                    e.printStackTrace();
                    ++mTotalLoss;
                    ++mLastLoss;
                }
            }
        });
    }
    public void sendMessage_AvoidMode(int code, int value, int type, byte[] data) {
        byte[] byteSend = cmd2Byte(code, value, type, data);

        mThreadPool.execute(new Runnable() {
            @Override
            public void run() {
                try {
                    InetAddress targetAddress = InetAddress.getByName(MainApp.getIP_AvoidMode());
                    DatagramPacket packet = new DatagramPacket(byteSend, byteSend.length, targetAddress, Integer.valueOf(MainApp.PORT_AVOIDMODE));
                    mSendSocket_AvoidMode.send(packet);
                    Log.d(TAG, "数据发送成功 : " + targetAddress + " : " + MainApp.PORT + "----" + HexUtils.encodeHexStr(byteSend));
                } catch (UnknownHostException e) {
                    e.printStackTrace();
                } catch (IOException e) {
                    e.printStackTrace();
                }
            }
        });
    }
    public void onRockerSafeClicked(RockerSafeView view, int mode){
        Log.i("[Rocker]", "Mode = " + mode);
        int code = 0x00;
        int value = 0x00;
        int type = 0x00;
        // TODO
//        if (view == mRockerViewDirction){
//            if (mode == 0x00) { // 后退
//                code = INST_WALK_X;value = -6553*4;
//            }
//            else if (mode == 0x01) { // 左走
//                code = INST_WALK_Y;value = -(24576 + 4000);
//            }
//            else if (mode == 0x02) { // 前进
//                code = INST_WALK_X;value = 6553 * 4;
//            }
//            else if (mode == 0x03) { // 右走
//                code = INST_WALK_Y;value = 24576 + 4000;
//            }
//        }


        sendMessage(code, value, type, null);
    }
    public void onRockerButtonClicked(RockerButtonView view, int mode){
        Log.i("[Rocker]", "Mode = " + mode);
        int code = 0x00;
        int value = 0x00;
        int type = 0x00;
        if (view == mRockerViewState){
            if (mode == 0x00) { // 起立 -> 起立/趴下
                code = INST_STAND_UP_DOWN;value = 0x00;
            }
            else if (mode == 0x01) { // 行走(开始运动）-> 开始运动和停止运动
                code = INST_MOTION_START_STOP;value = 0xFFFF_FFFF;
            }
            else if (mode == 0x02) { // 停止运动 -> 力控
                code = INST_FORCE_CONTROL_MODE;value = 0x0;
            }
            else if (mode == 0x03) { // 趴下 —> 行走/跑步
                code = INST_GAIT_WALK; value = 0x00;
            }
        }

        sendMessage(code, value, type, null);
    }
    private void startHeartbeatTimer() {
        mHeartBeatTimer.startTimer(100, MainApp.HeartBeatInterval);
    }
    class RecvUDPThread implements Runnable {
        @SuppressLint({"SetTextI18n", "UseCompatLoadingForDrawables"})
        @Override
        public void run() {
            byte[] data = new byte[1024];
            Log.i("[RecvUDPThread]", "Come");
            while (mRecieving) {
                try {
                    DatagramPacket packet = new DatagramPacket(data, data.length);
                    mRecvSocket.receive(packet);
                    byte[] recv = packet.getData();
                    int offset = packet.getOffset();
                    int len = packet.getLength();
                    if (len < 12)
                        continue;

                    int code = BufferUtil.readInt32LE(recv, offset);
                    int value = BufferUtil.readInt32LE(recv, offset + 4);
                    int type = BufferUtil.readInt32LE(recv, offset + 8);
                    if (code == 0x1008)
                        Log.i("[Recv]", "Code = " + Integer.toHexString(code) + " value = " + value + "type = " + type);
                    // Status. 必须为96字节
                    if (code ==  0x0902 && type == 0x01 && value == 0x60){
                        mRecvTimestamp = System.currentTimeMillis();
                        int doubleLen = value / 8;
                        double status[] = new double[doubleLen];
                        for (int n = 0; n < doubleLen; ++n){
                            double state = BufferUtil.readDouble(recv, offset + 12 + n * 8);
                            Log.i("[Recv]", "double_" + n + " = " + state);
                            status[n] = state;
                        }
                    }
                    // Battery. 必须为39字节
                    else if (code ==  0x21050F0A && type == 0x01 && value == 0x27){
                        mRecvTimestamp = System.currentTimeMillis();
//                        uint16_t voltage;
//                        int16_t current;
//                        uint16_t remaining_capacity;
//                        uint16_t nominal_capacity;
//                        uint16_t cycles;
//                        uint16_t production_date;
//                        uint16_t balanced_low;
//                        uint16_t balanced_high;
//                        uint16_t protected_state;
//                        uint8_t software_version;
//                        uint8_t battery_level;
//                        uint8_t mos_state;
//                        uint8_t battery_quantity;
//                        uint8_t battery_ntc;
//                        float battery_temperature[4];
                        MainApp.CurrentBatteryState.voltage = BufferUtil.readUInt16LE(recv, 12 + 0);
                        MainApp.CurrentBatteryState.current = BufferUtil.readUInt16LE(recv, 12 + 2);
                        MainApp.CurrentBatteryState.remaining_capacity = BufferUtil.readUInt16LE(recv, 12 + 4);
                        MainApp.CurrentBatteryState.nominal_capacity = BufferUtil.readUInt16LE(recv, 12 + 6);
                        MainApp.CurrentBatteryState.cycles = BufferUtil.readUInt16LE(recv, 12 + 8);
                        MainApp.CurrentBatteryState.production_date = BufferUtil.readUInt16LE(recv, 12 + 10);
                        MainApp.CurrentBatteryState.balanced_low = BufferUtil.readUInt16LE(recv, 12 + 12);
                        MainApp.CurrentBatteryState.balanced_high = BufferUtil.readUInt16LE(recv, 12 + 14);
                        MainApp.CurrentBatteryState.protected_state = BufferUtil.readUInt16LE(recv, 12 + 16);
                        MainApp.CurrentBatteryState.software_version = BufferUtil.readUInt8LE(recv, 12 + 18);
                        MainApp.CurrentBatteryState.battery_level = BufferUtil.readUInt8LE(recv, 12 + 19);
                        MainApp.CurrentBatteryState.mos_state = BufferUtil.readUInt8LE(recv, 12 + 20);
                        MainApp.CurrentBatteryState.battery_quantity = BufferUtil.readUInt8LE(recv, 12 + 21);
                        MainApp.CurrentBatteryState.battery_ntc = BufferUtil.readUInt8LE(recv, 12 + 22);
                        MainApp.CurrentBatteryState.battery_temperature[0] = BufferUtil.byte2float(recv, 12 + 23);
                        MainApp.CurrentBatteryState.battery_temperature[1] = BufferUtil.byte2float(recv, 12 + 27);
                        MainApp.CurrentBatteryState.battery_temperature[2] = BufferUtil.byte2float(recv, 12 + 31);
                        MainApp.CurrentBatteryState.battery_temperature[3] = BufferUtil.byte2float(recv, 12 + 35);

                        Log.i("[Recv]", "battery_level = " + MainApp.CurrentBatteryState.battery_level + "  voltage = "
                                + MainApp.CurrentBatteryState.voltage
                                + " current = " + MainApp.CurrentBatteryState.current
                                + " mos_state = " + MainApp.CurrentBatteryState.mos_state
                                + " software_version = " + MainApp.CurrentBatteryState.software_version);
                        DogControllerActivity.this.runOnUiThread(() -> {
                            updateBatteryState();
                        });
                    }
                    else if (code == 0x1009 && type == 0x01 && value == 0x3c) {
                        mRecvTimestamp = System.currentTimeMillis();
                        MainApp.CurrentMotionState.BasicState = BufferUtil.readUInt8LE(recv, 12 + 0);
                        MainApp.CurrentMotionState.GaitState = BufferUtil.readUInt8LE(recv, 12 + 1);
                        MainApp.CurrentMotionState.MaxForwardVel = BufferUtil.byte2float(recv, 12 + 4);
                        MainApp.CurrentMotionState.MaxBackwardVel = BufferUtil.byte2float(recv, 12 + 8);
                        MainApp.CurrentMotionState.LegOdonPos[0] = BufferUtil.byte2float(recv, 12 + 12);
                        MainApp.CurrentMotionState.LegOdonPos[1] = BufferUtil.byte2float(recv, 12 + 16);
                        MainApp.CurrentMotionState.LegOdonPos[2] = BufferUtil.byte2float(recv, 12 + 20);
                        MainApp.CurrentMotionState.LegOdonVel[0] = BufferUtil.byte2float(recv, 12 + 24);
                        MainApp.CurrentMotionState.LegOdonVel[1] = BufferUtil.byte2float(recv, 12 + 28);
                        MainApp.CurrentMotionState.RobotDistance = BufferUtil.byte2float(recv, 12 + 32);
                        MainApp.CurrentMotionState.LegOdonVel[2] = BufferUtil.byte2float(recv, 12 + 36);
                        MainApp.CurrentMotionState.TouchState = BufferUtil.readInt32LE(recv, 12 + 40);
                        int ControlStateBit = BufferUtil.readInt32LE(recv, 12 + 44);
                        MainApp.CurrentMotionState.NarrowWalk = (ControlStateBit & 0x01) == 0x01;
                        MainApp.CurrentMotionState.PosSafeFlag = (ControlStateBit & 0x02) == 0x02;
                        MainApp.CurrentMotionState.JointLimitFlag = (ControlStateBit & 0x04) == 0x04;
                        MainApp.CurrentMotionState.AutoChargeState = BufferUtil.readUInt8LE(recv, 12 + 48);
                        MainApp.CurrentMotionState.PosCtrlState = BufferUtil.readUInt8LE(recv, 12 + 49);

                    }
                    else if (code == 0x1008 && type == 0x01 && value == 88) { // Fix by chenmy from 0x49 to 88
                        // Note: long takes 8 bytes, char[15] takes 16 bytes, uint8_t[10] takes 12bytes
                        // Note: 12 is for header. 72
                        mRecvTimestamp = System.currentTimeMillis();
                        MainApp.CurrentRcsData.rcs_state[0] = (char) recv[12 + 72];
                    }
                } catch (IOException e) {
                    e.printStackTrace();
                }
            }

            Log.i("[UpdateECGViewThread]", "Break");
        }
    }
    void updateBatteryState(){
        int battery_level = MainApp.CurrentBatteryState.battery_level;
        mTextBattery.setText(battery_level + "%");
        if (battery_level < 5){
            mBtnBattery.setBackground(getResources().getDrawable(R.drawable.battery0));
        }else if(battery_level < 25){
            mBtnBattery.setBackground(getResources().getDrawable(R.drawable.battery1));
        }else if(battery_level < 45){
            mBtnBattery.setBackground(getResources().getDrawable(R.drawable.battery2));
        }else if(battery_level < 65){
            mBtnBattery.setBackground(getResources().getDrawable(R.drawable.battery3));
        }else if(battery_level < 85){
            mBtnBattery.setBackground(getResources().getDrawable(R.drawable.battery4));
        }else{
            mBtnBattery.setBackground(getResources().getDrawable(R.drawable.battery));
        }
    }
    void setFragmentVisible(BaseFragment fragment, boolean bVisible){
        FragmentTransaction transaction = getSupportFragmentManager().beginTransaction();
        if (bVisible){
            transaction.show(fragment);
        }else {
            transaction.hide(fragment);
        }
        transaction.commit();
    }
    void onNetOK() { setFragmentVisible(mFragmentNet,false); }
    void onNetCancel() { setFragmentVisible(mFragmentNet,false); }
    void onStatusOK(){
        setFragmentVisible(mFragmentStatus,false);
    }
    void onStatusCancel(){
        setFragmentVisible(mFragmentStatus,false);
    }
}
